#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <iostream>
#include <fstream>
#include <vector>

int main(int argc, char** argv)
{
  setlocale(LC_ALL,"");
  ros::init(argc, argv, "measure_pub");

  ROS_INFO("measure_pub start!");

  // 从参数获取文件名
  ros::NodeHandle n_param("~");
  std::string strFileName;
  std::string strMeasureTopic;
  n_param.param<std::string>("file_name", strFileName, "data");
  n_param.param<std::string>("topic", strMeasureTopic, "lidar_0");
    
  // 生成完整路径
  std::string strFile;
  char const* home = getenv("HOME");
  strFile = home;
  strFile += "/";
  strFile += strFileName;
  strFile += ".txt";
  ROS_WARN(strFile.c_str()); 

  // 初始化变量
  float val_max = 0;
  float val_min = 0;

  // 从文件获取数值
  std::vector<double> values;
  int values_size = 0;
  std::ifstream file(strFile);
  if (file.is_open())
  {

      std::string line;
      while (std::getline(file, line))
      {
          double value = std::stod(line);
          values.push_back(value);
      }

      file.close();

      values_size = values.size();
      ROS_WARN("从文件中读取了 %d 个测距值",values_size);
  }
  else
  {
      std::cerr << "无法打开文件" << std::endl;
      return 1;
  }

  // 发布话题
  ros::NodeHandle n;
  ros::Publisher mes_pub = n.advertise<std_msgs::Float64>(strMeasureTopic, 100);

  for(int i=5;i>=0;i--)
  {
    ROS_WARN("倒计时 - %d",i);
    sleep(1);
  }

  std_msgs::Float64 msg;
  int index = 0;

  ros::Rate r(2);
  while(ros::ok())
  {
    if(index < values_size)
    {
      float cur_val = values[index];
      msg.data = cur_val;
      mes_pub.publish(msg);
      ROS_INFO("发布第 %d 个测距值 %.2f",index,cur_val);
      if(index == 0)
      {
        val_max = val_min = cur_val;
      }
      if(cur_val != 0) // 部分雷达会出现0值，剔除掉
      {
        if(cur_val > val_max)
          val_max = cur_val;
        if(cur_val < val_min)
          val_min = cur_val;
      }
      index ++;
    }
    else
    {
      // 已经遍历结束，该显示浮动误差了
      if(index == values_size)
      {
        float val_diff = val_max - val_min;
        ROS_WARN("%s 的误差浮动为 %f",strMeasureTopic.c_str(),val_diff);
        index ++;
      }
    }
    ros::spinOnce();
    r.sleep();
  }

  return 0;
}